/*
 * Copyright (c) 2011, Georgia Tech Research Corporation
 * 
 * Humanoid Robotics Lab      Georgia Institute of Technology
 * Director: Mike Stilman     http://www.golems.org
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 *     * Redistributions of source code must retain the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer.
 *     * Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials
 *       provided with the distribution.
 *     * Neither the name of the Georgia Tech Research Corporation nor
 *       the names of its contributors may be used to endorse or
 *       promote products derived from this software without specific
 *       prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS
 * IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL GEORGIA
 * TECH RESEARCH CORPORATION BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */


#include <stdio.h>
#include <tchar.h>
#include <iostream>

#include "cv.h"
#include "highgui.h"
#include "cxcore.h"
using namespace cv;


double distance(Point one, Point two) {
	double square = pow((one.x - two.x), 2.0) + pow((one.y - two.y), 2.0);
	return sqrt(square);
}

int main ( int argc, char **argv )
{
	Point pleo;
	
	// Track PLEO
	vector<Point> pleoLocations;

	VideoCapture camera(0); // video source for webcam
	camera.set(CV_CAP_PROP_FRAME_WIDTH,640);
	camera.set(CV_CAP_PROP_FRAME_HEIGHT,480);
	// slices matrcies that hold H,S and V
	vector<Mat> slices;
	// create matrices to hold image
	Mat camImage;		// raw image from webcam
	Mat hsvImage;		// HSV image
	Mat blurImage;		// blur image
	Mat hue;			// hue channel
	Mat hue1;			// Hue upper bound
	Mat hue2;			// Hue lower bound
	Mat hue3;			// hue color filtering
	Mat final;			// Final display image

	// Theshold Settings
	//int HuethresL = 30, HuethresH = 71, blurSize = 1;  //Green
	//int HuethresL = 39, HuethresH = 74, blurSize = 1;  //pleo dark Green
	//int HuethresL = 50, HuethresH = 82, blurSize = 1;  //pleo light Green
	//int HuethresL = 50, HuethresH = 70, blurSize = 1;  //pleo
	//int HuethresL = 46, HuethresH = 62, blurSize = 1;  //pleo

	//int HuethresL = 85, HuethresH = 100, blurSize = 3;

	int HuethresL =116, HuethresH = 160, blurSize = 1; //Red
	
	// Initialize blob detector
	SimpleBlobDetector::Params params;
    params.minThreshold = 40;
    params.maxThreshold = 60;
    params.thresholdStep = 5;
    params.minArea = 300; // 1000
    params.minConvexity = .4f;
    params.minInertiaRatio = .1f;
    params.maxArea = 50000; // not 8000
    params.maxConvexity = 2;
    params.filterByColor = false;
    params.filterByCircularity = false;
    SimpleBlobDetector blobDetector( params );
    blobDetector.create("SimpleBlob");

	vector<KeyPoint> keyPoints;
	Mat diff;
	Mat frame;
	int delay = 5;

	boolean foundPLEO = false;

	while (!foundPLEO) {
		for (int i = 0; i < 20; i++) {
			camera >> camImage;
		}
		camImage.copyTo(frame);
	
		// blur image
		blur(camImage, blurImage, Size(2,2));
		// convert raw image to hsv
		cvtColor (camImage, hsvImage, CV_RGB2HSV);
		blur(hsvImage, hsvImage, Size(3,3));
		// split image to H,S and V images
		split(hsvImage,slices);
		slices[0].copyTo(hue); // get the hue channel
		threshold (hue,hue1,HuethresL,255, CV_THRESH_BINARY); // get lower bound
		threshold (hue,hue2,HuethresH,255, CV_THRESH_BINARY_INV); // get upper bound
		hue3 = hue1 & hue2; // multiply to get color range

		//Just to show we can access the keyPoints array directly
		//You can also investigate the properties of the features to determine their _size_
		camImage.copyTo(final);
		blobDetector.detect(hue3, keyPoints);
		if (keyPoints.size() > 0) {
			pleoLocations.push_back(keyPoints[0].pt);
			circle(camImage, keyPoints[0].pt, 20, cvScalar(255,0,0), 10);
			foundPLEO = true;
		}

		imshow("Looking for PLEO...", camImage);
	}
	
	// Calculated trajectory
	Point tmp = pleoLocations.back();
	Point bottomLeft (tmp.x, tmp.y);
	Point topRight (tmp.x+400,tmp.y-400);

	int lostPleo = 0;

	for(;;)
	{
		frame = camImage.clone();
		camera >> camImage;

		
		// blur image
		blur(camImage, blurImage, Size(2,2));
		// convert raw image to hsv
		cvtColor (camImage, hsvImage, CV_RGB2HSV);
		blur(hsvImage, hsvImage, Size(3,3));
		// split image to H,S and V images
		split(hsvImage,slices);
		slices[0].copyTo(hue); // get the hue channel
		threshold (hue,hue1,HuethresL,255, CV_THRESH_BINARY); // get lower bound
		threshold (hue,hue2,HuethresH,255, CV_THRESH_BINARY_INV); // get upper bound
		hue3 = hue1 & hue2; // multiply to get color range

		//Just to show we can access the keyPoints array directly
		//You can also investigate the properties of the features to determine their _size_
		camImage.copyTo(final);
        blobDetector.detect(hue3, keyPoints);

		if (keyPoints.size() == 0) {
			lostPleo++;
		} else {
			lostPleo = 0;
		}

		Point possible = 0;
		double minDist = 120;
		boolean addPoint = false;
		for(int i=0; i < keyPoints.size(); i++){
			circle(final, keyPoints[i].pt, 20, cvScalar(255,0,0), 10);
			double dist = distance(pleoLocations.back(), keyPoints[i].pt);

			if (dist < minDist) {
				addPoint = true;
				minDist = dist;
				possible = keyPoints[i].pt;
			}
		}
		if (addPoint) {
			pleoLocations.push_back(possible);
			lostPleo = 0;
		} else {
			lostPleo++;
			if (lostPleo > 5) {
				minDist = 250.0;
				for (int i = 0; i < keyPoints.size(); i++) {
					double dist = distance(pleoLocations.back(), keyPoints[i].pt);
					if (dist < minDist) {
						minDist = dist;
						possible = keyPoints[i].pt;
						lostPleo = 0;
					}
				}
				if (lostPleo == 0) {
					pleoLocations.push_back(possible);
				}
			}
		}

		if (pleoLocations.size() >= 2) {
			for (int i = 0; i < pleoLocations.size()-1; i++) {
				line(final, pleoLocations[i], pleoLocations[i+1], cvScalar(0,255,0));
			}
		}
		
		rectangle(final, bottomLeft, topRight, cvScalar(255,0,0));
		
		// show images
		imshow("Webcam Orignal", camImage);
		imshow("Hue color",hue3);
		imshow("Final",final);

        if( cvWaitKey(delay) >= 0 )
            break;
	}
	cvDestroyAllWindows();
}